﻿// physics vehicle, is a terrain physics object, and we are able controll the particle forces on input

using System;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Input;
using System.Diagnostics;
using FarseerGames.FarseerPhysics.Collisions;
using FarseerGames.FarseerPhysics;




namespace Organized
{

    public class PhysicsVehicle : PhysicsTerrainActor
    {

        //vehicle settings
        private float engineMaxForce = 40f;
        private float engineForce = 0f;
        private float targetForce = 0f;
        private float accelaration = 0.7f;      
        private float vehicleTurnSpeed = .15f;
        private float BackWheelRadius = 13;
        private float FrontWheelRadius = 10;


        //particle emittors
        PSEmitter pEmitter; //dust left and right backside 
        PSEmitter pEmitterMudd; //wheel mudd sparks all wheels
        

        //model bone references
        Matrix BackWheelRollMatrix = Matrix.Identity;
        Matrix FrontWheelRollMatrix = Matrix.Identity;
        Matrix frontwheelTurnMatrix = Matrix.Identity;
        
        ModelBone leftBackWheelBone;
        ModelBone rightBackWheelBone;
        ModelBone leftFrontWheelBone;
        ModelBone rightFrontWheelBone;        
        ModelBone bodyBone;

        Matrix bodyTransform;
        Matrix bodyLag = Matrix.Identity;        
        Matrix leftBackWheelTransform;
        Matrix rightBackWheelTransform;
        Matrix leftFrontWheelTransform;
        Matrix rightFrontWheelTransform;

        //suspension bones
        //suspension parameters     
        ModelBone BackSuspension;
        ModelBone FrontSuspension;

        //strore the initial position of suspension
        Matrix BackSuspensionTransform;      
        Matrix FrontSuspensionTransform;

        //y position to set final suspensionheight    
        float BackSuspensionPoint;
        float FrontSuspensionPoint;
   

        float BackSuspensionPointSpeed;   
        float FrontSuspensionPointSpeed;


        // variable to move the suspension       
        Matrix BackSuspensionMatrix;
        Matrix FrontSuspensionMatrix;
     
        

        //suspension settings
        float maxSuspForce = 30f; // higher numbers make suspension distance longer
        float backWheelSuspSpringness = 0.95f; //higher numbers add more bounce
        float backWheelSpringLatency = 20f; // lower numbers make suspension react faster.
        float frontWheelSuspSpringness = 0.957f; //higher numbers add more bounce
        float frontWheelSpringLatency = 17f; // lower numbers make suspension react faster.

        float frame_length = 100f;
       
        float wheelTurnSpeed = 0.3f;
        public Vector2 Velocity = Vector2.Zero; //our directional speed (x,y)
       
        float wheelRotation = 0; //how much our front wheels are turned

        float rotationalVelocity = 0; //keep track of how much we are turning
        bool BreaksOn = false;
        private float turnAmount = 0f;  //how much we are turning

        public PhysicsVehicle(Model Model, Lighting _light, PhysicsParticle _physicsParticle, Vector3 _position, GameScreen Parent)
            : base(Model,_light, _physicsParticle, _position, Parent)
        {
            floorAttached = false;
           
            //turn off angular velocity impacts
            copyParticleAngle = false;

            // Set correct wheel radius according to my scale (particleradius)
            BackWheelRadius *= (PhysicsParticle.Radius / 10);
            FrontWheelRadius *= (PhysicsParticle.Radius / 10);
            // Add vertical collision handler to prevent falling from top
            // AddCollisionHandler();

            //get the wheel and body bones
            leftBackWheelBone = Model.Bones["l_back_wheel_geo"];
            rightBackWheelBone = Model.Bones["r_back_wheel_geo"];
            leftFrontWheelBone = Model.Bones["l_front_wheel_geo"];
            rightFrontWheelBone = Model.Bones["r_front_wheel_geo"];
            bodyBone = Model.Bones["body"];
           
            //set wheel and body matrixes
            leftBackWheelTransform = leftBackWheelBone.Transform;
            rightBackWheelTransform = rightBackWheelBone.Transform;
            leftFrontWheelTransform = leftFrontWheelBone.Transform;
            rightFrontWheelTransform = rightFrontWheelBone.Transform;           
            bodyTransform = bodyBone.Transform;

            //get the suspension dummies         
            BackSuspension = Model.Bones["l_back_suspension"];
            FrontSuspension = Model.Bones["l_front_suspension"]; 

            BackSuspensionTransform = BackSuspension.Transform;            
            FrontSuspensionTransform = FrontSuspension.Transform;
       

            //Dust particles
            pEmitter = new PSEmitter(Position, new Vector3(0, 10, -14), Quaternion.CreateFromRotationMatrix(Rotation), "DustTrail", this.Parent);            
            pEmitterMudd = new PSEmitter(Position, new Vector3(0, 10, -14), Quaternion.CreateFromRotationMatrix(Rotation), "MuddFarts", this.Parent);
            // make sure particles are draw on top of scene for correct alpha blending
            pEmitter.DrawOrder = 7000;           
            pEmitterMudd.DrawOrder = 6000;

            frame_length *= this.Scale.X;

            FacingDirection = (float)MathUtil.DegreeToRadian(270f);

            //PhysicsParticle.Geom.CollisionCategories = CollisionCategory.Cat11;
           // PhysicsParticle.Geom.CollidesWith = CollisionCategory.Cat1;
        }

        
        

        public override void Update()
        {

            //Remove angular velocity from particle
            PhysicsParticle.Body.AngularVelocity = 0f;

            TurnWheels();

            CalculateSuspension();
           

            EmitParticles();

            base.Update();
        }

        private void CalculateSuspension(){

            ///SUSPENSION
            ///BACK WHEELS
            //get the height difference to apply force to suspension
            float suspForceY = Vspeed;
            //increase and invert force
            suspForceY *= -10;
            //thresshold force to simulate max stretch
            suspForceY = MathHelper.Clamp(suspForceY, -maxSuspForce, maxSuspForce);

            //make vector point 'follow' the suspForceY position in a spring like behavior
            BackSuspensionPointSpeed += (BackSuspensionPoint - suspForceY) / backWheelSpringLatency;
            BackSuspensionPoint -= BackSuspensionPointSpeed;
            //always decrease suspension to stop spring from, ..springing?
            BackSuspensionPointSpeed *= backWheelSuspSpringness;
            //set suspension targets            
            BackSuspensionMatrix = Matrix.CreateTranslation(new Vector3(0,BackSuspensionPoint, 0));
            

            ///FRONT WHEELS        

            //make vector point 'follow' the suspForceY position in a spring like behavior
            FrontSuspensionPointSpeed += (FrontSuspensionPoint - suspForceY) / frontWheelSpringLatency;
            FrontSuspensionPoint -= FrontSuspensionPointSpeed;
            //always decrease suspension to stop spring from, ..springing?
            FrontSuspensionPointSpeed *= frontWheelSuspSpringness;
            //set suspension targets            
            FrontSuspensionMatrix = Matrix.CreateTranslation(new Vector3(0, FrontSuspensionPoint, 0));

          

            //USE THIS INFO TO CALCULATE BODY LAGGING according to suspensionMatrixes
            //calculate angle between front and back for Z rotation
            double dpos = FrontSuspensionPoint - BackSuspensionPoint;
            double suspAngleFrontBack = Math.Atan2(dpos, frame_length);
            //calculate average height between front and back for y offset
            float suspDiffHeightFrontBack = ((FrontSuspensionPoint + BackSuspensionPoint) / 2);

            //bodyLag = Matrix.CreateRotationX(-0.2f*turnAmount);
            bodyLag = Matrix.CreateTranslation(0, 0, suspDiffHeightFrontBack);
            bodyLag *= Matrix.CreateRotationX((float)suspAngleFrontBack);

        }


        private void EmitParticles()
        {
            if (!Airborne && distanceToMove != 0)
            {
                float rotForce = rotationalVelocity * 10;
                if(rotForce < 0){rotForce *= -1;}


                float pAlpha = (distanceToMove / 30) + rotForce;
                //Emit DUST 
                    //left back
                    pEmitter.myPosOffset.X = this.Scale.X * -4.5f - 10f;
                    pEmitter.UpdateSettings(Position,  orientation);                              
                    pEmitter.EmitParticle(0, pAlpha, 25f);


                    //right back
                    pEmitter.myPosOffset.X = -pEmitter.myPosOffset.X;
                    pEmitter.UpdateSettings(Position, orientation);  
                    pEmitter.EmitParticle(0, pAlpha, 25f);

                
                


            }
        }
        float distanceToMove;
        private void TurnWheels()
        {

            ///CALCULATE MOVED DISTANCE TO REFERENCE WHEEL ROTATION
            distanceToMove = Vector3.Distance(Position, oldPosition);
            //moving forward or backwards ?
            int rollDirection = engineForce > 0 ? 1 : -1;
            float theta;

         
                theta = distanceToMove / BackWheelRadius;
                BackWheelRollMatrix *= Matrix.CreateRotationZ(theta * rollDirection);
       
            //rotate smaller front wheels
            theta = distanceToMove / FrontWheelRadius;
            FrontWheelRollMatrix *= Matrix.CreateRotationZ(theta * rollDirection);
        }

       


        public override void Draw()
        {
            //transform wheel bones
            leftBackWheelBone.Transform = BackWheelRollMatrix * leftBackWheelTransform;
            rightBackWheelBone.Transform = BackWheelRollMatrix * rightBackWheelTransform;
            leftFrontWheelBone.Transform = FrontWheelRollMatrix * frontwheelTurnMatrix * leftFrontWheelTransform;
            rightFrontWheelBone.Transform = FrontWheelRollMatrix * frontwheelTurnMatrix * rightFrontWheelTransform;

            //transform body
            bodyBone.Transform = bodyLag * bodyTransform;
            BackSuspension.Transform = BackSuspensionMatrix * BackSuspensionTransform;
            FrontSuspension.Transform = FrontSuspensionMatrix * FrontSuspensionTransform;
         

            base.Draw();
        }


        public Vector2 myRotationVector;
        Vector2 directionForceVector;

        public void HandleInput(KeyboardState currentKeyboardState)
        {
            
            
            

               
            // myRotation -= (float)MathUtil.DegreeToRadian(45f);

            myRotationVector = MathUtil.RadianToVector2(FacingDirection);
                   
                
            //somethings wrong, correct vector
            directionForceVector = new Vector2(myRotationVector.Y, myRotationVector.X) * engineForce;
         
            float TargetTurnAmount = 0;

            //you cant turn if we are airborne
            if (!Airborne )
            {

                //test for brakes:
                if (currentKeyboardState.IsKeyDown(Keys.Space))
                {
                    if (!BreaksOn)
                    {
                        
                        BreaksOn = true;
                    }
                   
                    engineForce *= 0.6f;
                }
                else
                {
                    if (BreaksOn)
                    {
                       
                        BreaksOn = false;
                    }
                }
            }

                if (currentKeyboardState.IsKeyDown(Keys.Left))
                {
                    if (!BreaksOn)
                    {
                        TargetTurnAmount = 1;
                    }
                    else
                    {
                        TargetTurnAmount = 1;
                    }
                }

                if (currentKeyboardState.IsKeyDown(Keys.Right))
                {
                    if (!BreaksOn)
                    {
                        TargetTurnAmount = -1;
                    }
                    else
                    {
                        TargetTurnAmount = -1;
                    }
                }

                //bind between positive
                FacingDirection = (float)MathUtil.ThressHoldAngle(FacingDirection);
            

           
            //turn amount relative to our speed, so we turn less at slower speed (and not at all at zero speed)
            //TargetTurnAmount *= ZSpeed;
          

            turnAmount += (TargetTurnAmount - turnAmount) / 8;

            //turn front wheels
            wheelRotation += ((TargetTurnAmount) - wheelRotation) * wheelTurnSpeed;
            frontwheelTurnMatrix = Matrix.CreateRotationX(0.2f * wheelRotation);


            if (!Airborne)
            {
                //turn vehicle according to turn amount and turnspeed setting

                rotationalVelocity = turnAmount * vehicleTurnSpeed;

            }
            else
            {
                //slowly stop rotating
                rotationalVelocity *= 0.97f;
            }
            FacingDirection += rotationalVelocity;

                if (currentKeyboardState.IsKeyDown(Keys.Up))
                {
                    //angular velocity
                    //get the particle rotation and convert to vector2                    
                    targetForce = engineMaxForce;
                    
                }

                else if (currentKeyboardState.IsKeyDown(Keys.Down))
                {
                    //angular velocity
                    targetForce = -engineMaxForce;                    
                }
                else
                {
                    targetForce *= 0.9f;
                }

                engineForce += (targetForce - engineForce) * accelaration;

                vehicleTurnSpeed = (engineForce/engineMaxForce)/20;

                  

                if (!Airborne)
                {
                    PhysicsParticle.Body.ApplyForceAtLocalPoint(ref directionForceVector, ref myRotationVector);
                }
               
                Matrix rotationMatrix = Matrix.CreateRotationY(FacingDirection);
                Rotation = rotationMatrix;

            
        }





    }


}